/*
 * @brief Sensor Hub interface module (implementation file)
 *
 * @note
 * Copyright(C) NXP Semiconductors, 2015
 * All rights reserved.
 *
 * @par
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * LPC products.  This software is supplied "AS IS" without any warranties of
 * any kind, and NXP Semiconductors and its licensor disclaim any and
 * all warranties, express or implied, including all implied warranties of
 * merchantability, fitness for a particular purpose and non-infringement of
 * intellectual property rights.  NXP Semiconductors assumes no responsibility
 * or liability for the use of the software, conveys no license or rights under any
 * patent, copyright, mask work right, or any other intellectual property rights in
 * or to any products. NXP Semiconductors reserves the right to make changes
 * in the software without notification. NXP Semiconductors also makes no
 * representation or warranty that such application will be suitable for the
 * specified use without further testing or modification.
 *
 * @par
 * Permission to use, copy, modify, and distribute this software and its
 * documentation is hereby granted, under NXP Semiconductors' and its
 * licensor's relevant copyrights in the software, without fee, provided that it
 * is used in conjunction with NXP Semiconductors microcontrollers.  This
 * copyright, permission, and disclaimer notice must appear in all copies of
 * this code.
 */

#include <stdio.h>
#include <process.h>
#include <stdint.h>
#include <stdbool.h>
#include <windows.h>

#include "lpcusbsio.h"
#include "hostif_protocol.h"

#include "trace.h"
#include "auto_rotate.h"
#include "gl_text.h"
#include "sensors.h"
#include "teapot.h"

#include "stat_acc.h"
#include "i2c_io.h"
#include "sensorhal.h"

/*****************************************************************************
 * Private types/enumerations/variables
 ****************************************************************************/
static uint8_t		m_ParseBuff[PARSE_BUFF_SZ];
static HANDLE		m_sioMutex		= NULL;
static HANDLE		m_dataCapture	= NULL;
static uint32_t		m_sensor		= 0;
static bool			m_connect		= false;
static bool			m_no_sensors	= false;
static bool			m_use_batch		= true;

#define	SENSOR_IRQ_PORT		1
#define	SENSOR_IRQ_BIT		2

//------------------------------------------------------------------------------
// This structure must match the sensor IDs in hostif_protocol.h exactly.
// even though the BSXLite library only supports a subset of the sensors.
// Here is the exact list:
//
// - LPCSH_SENSOR_ID_ACCELEROMETER
// - LPCSH_SENSOR_ID_ACCELEROMETER_UNCALIBRATED
// - LPCSH_SENSOR_ID_MAGNETIC_FIELD
// - LPCSH_SENSOR_ID_MAGNETIC_FIELD_UNCALIBRATED
// - LPCSH_SENSOR_ID_GYROSCOPE
// - LPCSH_SENSOR_ID_GYROSCOPE_UNCALIBRATED
// - LPCSH_SENSOR_ID_LIGHT
// - LPCSH_SENSOR_ID_PRESSURE (includes temperature)
// - LPCSH_SENSOR_ID_PROXIMITY
// - LPCSH_SENSOR_ID_ORIENTATION
// - LPCSH_SENSOR_ID_ROTATION_VECTOR
//
//	All other sensors should be marked as un-supported.
//------------------------------------------------------------------------------
static const uint32_t m_sensorDataLen[] =
{
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 00: LPCSH_SENSOR_ID_ACCELEROMETER
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 01: LPCSH_SENSOR_ID_ACCELEROMETER_UNCALIBRATED
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 02: LPCSH_SENSOR_ID_MAGNETIC_FIELD
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 03: LPCSH_SENSOR_ID_MAGNETIC_FIELD_UNCALIBRATED
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 04: LPCSH_SENSOR_ID_GYROSCOPE
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 05: LPCSH_SENSOR_ID_GYROSCOPE_UNCALIBRATED
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 06: LPCSH_SENSOR_ID_LIGHT
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 07: LPCSH_SENSOR_ID_PRESSURE
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 08: LPCSH_SENSOR_ID_PROXIMITY
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 09: LPCSH_SENSOR_ID_RELATIVE_HUMIDITY
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 10: LPCSH_SENSOR_ID_AMBIENT_TEMPERATURE
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 11: LPCSH_SENSOR_ID_GRAVITY
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_motion_sensor_node),						// 12: LPCSH_SENSOR_ID_LINEAR_ACCELERATION
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_orientation_node),						// 13: LPCSH_SENSOR_ID_ORIENTATION
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_quaternion_node),							// 14: LPCSH_SENSOR_ID_ROTATION_VECTOR
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_quaternion_node),							// 15: LPCSH_SENSOR_ID_GEOMAGNETIC_ROTATION_VECTOR
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_quaternion_node),							// 16: LPCSH_SENSOR_ID_GAME_ROTATION_VECTOR
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_step_sensor_node),						// 17: LPCSH_SENSOR_ID_STEP_DETECTOR
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_step_sensor_node),						// 18: LPCSH_SENSOR_ID_STEP_COUNTER
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_significant_motion_node),					// 19: LPCSH_SENSOR_ID_SIGNIFICANT_MOTION
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_activity_classification_state_node),		// 20: LPCSH_SENSOR_ID_ACTIVITY_CLASSIFICATION_STATE
	0,																										// 21: LPCSH_SENSOR_ID_AUG_REALITY_COMPASS
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_tap_sensor_node),							// 22: LPCSH_SENSOR_ID_TAP_DETECTOR
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_shake_sensor_node),						// 23: LPCSH_SENSOR_ID_SHAKE_DETECTOR
	0,																										// 24: LPCSH_SENSOR_ID_STEP_SEGMENT_DETECTOR
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct ShVoiceTriggerWriteUserDataCommand_t),			// 25: LPCSH_SENSOR_ID_VOICE_TRIGGER
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_ir_blaster_rx_node),						// 26: LPCSH_SENSOR_ID_IR_BLASTER_RX_PACKET
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_ir_blaster_status_node),					// 27: LPCSH_SENSOR_ID_IR_BLASTER_STATUS
	sizeof(struct lpcsh_sensor_node_header) + sizeof(struct lpcsh_meta_sensor_flush_done_node),				// 28: LPCSH_SENSOR_ID_META_SENSOR_FLUSH_DONE
	1																										// 29: LPCSH_SENSOR_ID_FAKE_SENSOR
};


#define TASK_LOOP	for (;;)
/*****************************************************************************
 * Public types/enumerations/variables
 ****************************************************************************/

/*****************************************************************************
 * Private functions
 ****************************************************************************/
static bool ParseSensorData(uint8_t* pData, uint16_t datalen)
{
	uint8_t sensor_id;
	uint32_t offset=0;
	uint8_t* sData = pData;

	while (datalen > offset) {
		sensor_id = *pData;
		switch (sensor_id) {
		case LPCSH_SENSOR_ID_ACCELEROMETER:
			accel_cal_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_ACCELEROMETER_UNCALIBRATED:
			accel_uncal_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_MAGNETIC_FIELD:
			mag_cal_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_MAGNETIC_FIELD_UNCALIBRATED:
			mag_uncal_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_GYROSCOPE:
			gyro_cal_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_GYROSCOPE_UNCALIBRATED:
			gyro_uncal_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_LIGHT:
			light_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_PRESSURE:
			pressure_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_PROXIMITY:
			proximity_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_ROTATION_VECTOR:
			teapot_data_acq(pData);
			rotation_data_acq(pData);
			break;

		case LPCSH_SENSOR_ID_FAKE_SENSOR:
			break;
			
		default:
			trace(TRACE_INFO, "sensor id out of range: %d", sensor_id);
			if (trace_level_get() & TRACE_LOGGING) {
				trace(TRACE_INFO, "- save pointer: 0x%08x", sData);
				trace(TRACE_INFO, "- data pointer: 0x%08x", pData);
				trace(TRACE_INFO, "- data offset: %d", offset);
				trace(TRACE_INFO, "- data length: %d", datalen);
				dump_data(sData, datalen);
				exit(1);
			}
			return false;
			break;
		}
		pData += m_sensorDataLen[sensor_id];
		offset += m_sensorDataLen[sensor_id];
	}
	return true;
}

static bool ReadSensorData(void)
{
	uint16_t	datalen;
	bool		r_val;

	datalen = i2c_sd_get_data_len();											/* get the data length */
	r_val = i2c_sd_get_data(m_ParseBuff, datalen);								/* get the data */
	if (r_val == true)															/* if the return value was true... */
		r_val = ParseSensorData(m_ParseBuff, datalen);							/* Parse data */

	return r_val;
}

static bool SensorIRQ(void)
{
	uint32_t	status;
	LPC_HANDLE	sio_port;
	int32_t		ret;

	sio_port = i2c_sd_get_sio_handle();											/* get a handle to the sio object */
	ret = GPIO_ReadPort(sio_port, SENSOR_IRQ_PORT, &status);					/* get the port status */
	if (ret < 0)																/* if there was an error... */
		trace(TRACE_INFO,"GPIO_ReadPort: %d",ret);								/* trace it */
	return ((status & (1 << SENSOR_IRQ_BIT)) == 0) ? true : false;				/* return the status of the i/o pin */
}

static void bgThreadFunc(void *pMyID)
{
	trace_enter();
	TASK_LOOP {
		WaitForSingleObject(m_dataCapture, INFINITE);							/* wait for loop enable */
		WaitForSingleObject(m_sioMutex, INFINITE);								/* get the serial I/O mutex */
		if (SensorIRQ() == true) ReadSensorData();								/* read the sensor */
		ReleaseMutex(m_sioMutex);												/* release the mutex */
		Sleep(10);																/* MS Sleep function: waits 10ms (100Hz) */
	}
	trace_exit();
}

static void connectThreadFunc(void *pMyID)
{
	trace_enter();																/* enter the function */
	auto_rotate_set(true);														/* enable auto rotate */

	TASK_LOOP {
		if (m_connect == false) {												/* are we connected ? */
			if (LPCUSBSIO_GetNumPorts(LPCUSBSIO_VID, LPCUSBSIO_PID) > 0) {
				trace(TRACE_INFO,"I2C connect");								/* trace it */
				i2c_sd_open();													/* open i2c port */
				_beginthread(bgThreadFunc, 0, 0);								/* start the background thread */
				sensor_hal_hub_reset();											/* reset firmware before enabling sensors */
				SetEvent(m_dataCapture);										/* start the background thread */
				auto_rotate_set(false);											/* disable auto rotate */
				m_connect = true;												/* system is connected! */
				trace_exit();													/* exit the function */
				_endthread();													/* all done */
			}
		}
		Sleep(200);																/* MS Sleep function: waits 200ms (50Hz)*/
	}
}

/*****************************************************************************
 * Public functions
 ****************************************************************************/
bool sensor_hal_init(void)
{
	trace_enter();																/* enter function */
	i2c_sd_init();																/* initialize I2C port */
	m_sioMutex = CreateMutex(NULL, FALSE, NULL);								/* Create SIO mutex */
	m_dataCapture = CreateEvent(NULL, TRUE, FALSE, NULL);
	trace_exit();																/* exit function */
	return true;
}

bool sensor_hal_open(int portType)
{
	trace_enter();																/* enter the function */
	m_connect = false;															/* system not connected */
	trace(TRACE_INFO,"Starting connect thread");								/* trace it */
	_beginthread(connectThreadFunc, 0, 0);										/* start thread */
	trace_exit();																/* trace the exit */
	return m_connect;															/* return connection status */
}

bool sensor_hal_enable(uint8_t sensor_id, bool enable)
{
	bool r_val = true;

	WaitForSingleObject(m_sioMutex, INFINITE);									/* get the serial I/O mutex */

	if (m_use_batch == true) {
		r_val = i2c_sd_sensor_batch(sensor_id, enable);							/* batch the sensor */
		Sleep(50);																/* wait 50ms */
	}
	r_val |= i2c_sd_sensor_enable(sensor_id, enable);							/* enable / disable the sensor */
	if (r_val == true) {														/* if operation went OK... */
		if (enable == true) {													/* if this was an enable... */
			m_sensor |= (1 << (sensor_id & 0x1F));								/* set the bit in the m_sensor word */
		} else {																/* if not... */
			m_sensor &= ~(1 << (sensor_id & 0x1F));								/* clear the bit */
		}
	}
	trace(TRACE_INFO, "sensors: %08x",m_sensor);
	ReleaseMutex(m_sioMutex);													/* release the mutex */

	return r_val;																/* return the status of the operation */
}

bool sensor_hal_who_am_i(void)
{
	WaitForSingleObject(m_sioMutex, INFINITE);									/* get the serial I/O mutex */
	i2c_sd_who_am_i();
	ReleaseMutex(m_sioMutex);													/* release the mutex */

	return true;
}

bool sensor_hal_hub_reset(void)
{
	WaitForSingleObject(m_sioMutex, INFINITE);									/* get the serial I/O mutex */
	i2c_sd_sensor_reset();														/* reset all sensors */
	ReleaseMutex(m_sioMutex);													/* release the mutex */
	m_sensor = 0;																/* clear out all sensors */
	if (m_no_sensors == false) {
		Sleep(50);																/* wait 50ms */
		sensor_hal_enable(LPCSH_SENSOR_ID_ROTATION_VECTOR, true);				/* enable the "teapot" sensor */
	}

	return true;
}

void sensor_hal_exit(void)
{
	i2c_sd_sensor_reset();														/* disable all sensors */
	if (trace_level_get() & TRACE_LOGGING) stat_acc_dump();						/* print buffer information (if trace level is set) */
}

bool sensor_hal_get_connect(void)			{	return m_connect;		}
void sensor_hal_set_connect(bool e)			{	m_connect = e;			}
bool sensor_hal_get_no_sensors(void)		{	return m_no_sensors;	}
void sensor_hal_set_no_sensors(bool e)		{	m_no_sensors = e;		}
bool sensor_hal_get_use_batch(void)			{	return m_use_batch;		}
void sensor_hal_set_use_batch(bool e)		{	m_use_batch = e;		}

